#include "game.h"

#include "VehicleApiUtils.h"
#include "VehicleSetup.h"

#include <Physics/Utilities/VisualDebugger/hkpPhysicsContext.h>

#include <Physics\Collide\Filter\Group\hkpGroupFilter.h>

#include <Common\Serialize\Util\hkLoader.h>
#include <Common\Serialize\Util\hkRootLevelContainer.h>
#include <Common\Base\System\Io\IStream\hkIStream.h>
#include <Common\Serialize\Util\hkSerializeUtil.h>

#include <Physics\Utilities\Serialize\hkpPhysicsData.h>

#include <Demos\DemoCommon\Utilities\Asset\hkAssetManagementUtil.h>

#include <Animation\Animation\hkaAnimationContainer.h>

#include <iostream>

static hkpRigidBody* getBodyFromFile(const char* file, const char* name) {
	hkSerializeUtil::ErrorDetails loadError;
	hkResource* loadedData = hkSerializeUtil::load(file, &loadError);
	if(loadedData) {
		hkRootLevelContainer* container = loadedData->getContents<hkRootLevelContainer>();
		if(container) {
			hkpPhysicsData* physicsData = reinterpret_cast<hkpPhysicsData*>(container->findObjectByType(hkpPhysicsDataClass.getName()));
			if(physicsData) {
				return physicsData->findRigidBodyByName(name);
			}
		}
	}
	return HK_NULL;
}

static inline void HK_CALL errorReport(const char* msg, void* userContext) {
	std::cerr<<msg<<std::endl;
}

Game::Game() : accelerate(-1), steer(-1) {
	//Initialize Havok
	hkMemoryRouter* memoryRouter = hkMemoryInitUtil::initDefault(hkMallocAllocator::m_defaultMallocAllocator, hkMemorySystem::FrameInfo(1024*1024));
	hkBaseSystem::init(memoryRouter, errorReport);
	{
		pad = new hkgPad();

		m_tag = 0;

		m_inputXPosition = 0.0f;
		m_inputYPosition = 0.0f;

		//Create the world
		{
			hkpWorldCinfo info;
			info.m_collisionTolerance = 0.1f;
			info.m_gravity.set(0.0f, -9.8f, 0.0f);
			info.setBroadPhaseWorldSize(1000.0f);
			info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM);
			m_world = new hkpWorld(info);
			m_world->lock();

			hkpAgentRegisterUtil::registerAllAgents(m_world->getCollisionDispatcher());
		}

		buildLandscape();
		buildWall();
		
		//create chassis
		hkpRigidBody* micra = getBodyFromFile("./media/micra/micra.hkx", "micra4");
		if(micra == HK_NULL)
			return;

		{
			hkpRigidBodyCinfo chassisInfo;
			micra->getCinfo(chassisInfo);
			//chassisInfo.m_friction = 0.4f;
			chassisInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
			chassisInfo.m_position.set(5.0f, 0.0f, 0.0f);

			//hkpMassProperties massProperties;
			//hkpInertiaTensorComputer::computeShapeVolumeMassProperties(chassisInfo.m_shape, 750.0f, massProperties);
			//chassisInfo.m_inertiaTensor = massProperties.m_inertiaTensor;
			//chassisInfo.m_centerOfMass = massProperties.m_centerOfMass;
			//chassisInfo.m_mass = massProperties.m_mass;
			//chassisInfo.m_collisionFilterInfo = hkpGroupFilter::calcFilterInfo(1, 0);
			micra->removeReference();
			micra = new hkpRigidBody(chassisInfo);
		}

		createVehicle(micra);
		micra->removeReference();

		createDisplayWheels();

		VehicleApiUtils::createCamera(m_camera);

		// Register all the physics viewers
		hkpPhysicsContext::registerAllPhysicsProcesses(); 

		// Set up a physics context containing the world for the use in the visual debugger
		hkpPhysicsContext* context = new hkpPhysicsContext;
		context->addWorld(m_world); 

		// Set up the visual debugger
		{
			// Setup the visual debugger
			hkArray<hkProcessContext*> contexts;
			contexts.pushBack(context);

			visualDebugger = new hkVisualDebugger(contexts);
			visualDebugger->serve();
		}

		m_world->unlock();
	}
}

Game::~Game() {
	m_world->lock();

	visualDebugger->removeReference();

	for(int i = 0; i < 10; i++) {
		if(m_wall[i]->isAddedToWorld())
			m_world->removeEntity(m_wall[i]);
	}

	m_vehicle->removeFromWorld();
	m_vehicle->removeReference();

	m_world->unlock();
	m_world->removeReference();
	hkBaseSystem::quit();
	hkMemoryInitUtil::quit();
}

void Game::resetVehicle() {
	m_world->lock();

	hkVector4 position = m_vehicle->getChassis()->getPosition();
	if(position(0) > 100)
		position(0) = 95;
	else if(position(0) < -100)
		position(0) = -95;

	if(position(1) < 0)
		position(1) = 1;

	if(position(2) > 100)
		position(2) = 95;
	else if(position(2) < -100)
		position(2) = -95;

	m_vehicle->getChassis()->setPositionAndRotation(position, hkQuaternion::getIdentity());
	m_vehicle->getChassis()->setLinearVelocity(hkVector4::getZero());
	m_vehicle->getChassis()->setAngularVelocity(hkVector4::getZero());

	m_world->unlock();
}

void Game::step() {
	{
		m_world->lock();
		for(int i = 0; i < 10; i++) {
			if(m_wall[i]->isAddedToWorld() && m_wall[i]->getPosition()(1) < -4.0f)
				m_world->removeEntity(m_wall[i]);
		}
		if(m_vehicle->getWorld() != HK_NULL && m_vehicle->getChassis()->getPosition()(1) < -4.0f) {
			this->resetVehicle();
		}
		m_world->unlock();
	}

	{
		m_world->lock();

		if(accelerate != -1 || steer != -1)
			m_vehicle->getChassis()->activate();

		VehicleApiUtils::steer(accelerate, steer, pad, m_inputXPosition, m_inputYPosition, *m_vehicle, FRAME_PERIOD);

		m_world->unlock();
	}

	{
		m_world->stepDeltaTime(FRAME_PERIOD);
		visualDebugger->step();
	}

	{
		m_world->lock();

		VehicleApiUtils::syncDisplayWheels(*m_vehicle, m_displayWheelId, m_tag);

		out = VehicleApiUtils::updateCamera(*m_vehicle->getChassis(), FRAME_PERIOD, m_camera);

		m_world->unlock();
	}
}

void Game::buildWall() {
	hkVector4 halfExtents(0.5f, 0.5f, 0.5f);
	hkpRigidBodyCinfo wallInfo;
	hkpShape* wallShape = new hkpBoxShape(halfExtents, 0);
	wallInfo.m_shape = wallShape;
	hkpMassProperties massProperties;
	wallInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
	hkpInertiaTensorComputer::computeShapeVolumeMassProperties(wallShape, 500.0f, massProperties);
	wallInfo.m_mass = massProperties.m_mass;
	wallInfo.m_centerOfMass = massProperties.m_centerOfMass;
	wallInfo.m_inertiaTensor = massProperties.m_inertiaTensor;
	wallInfo.m_friction = 0.7f;
	wallInfo.m_restitution = 0.4f;
	int k = 0;
	hkReal yPos = 0.0f;
	hkReal zPos = 0.0f;
	for(int i = 0; i < 2; i++) {
		for(int j = 0; j < 5; j++) {
			wallInfo.m_position.set(5, yPos, zPos);
			m_wall[k] = new hkpRigidBody(wallInfo);
			m_world->addEntity(m_wall[k]);
			m_wall[k]->removeReference();
			k++;
			zPos += 1.0f;
		}
		zPos = 0.0f;
		yPos += 1.0f;
	}
	wallShape->removeReference();
}

void Game::buildLandscape() {
	{
		//const hkReal sideLength = 100.0f;
		const hkReal sideLength = 250.0f;
		const hkReal depth = 0.01f;
		const hkReal width = 3.0f;
		const hkReal height = 5.0f;

		hkVector4 halfExtentsGround = hkVector4(sideLength, depth, sideLength);

		hkpRigidBodyCinfo groundInfo;
		groundInfo.m_motionType = hkpMotion::MOTION_FIXED;
		groundInfo.m_friction = 0.8f;

		//ground
		{
			groundInfo.m_shape = new hkpBoxShape(halfExtentsGround, 0);
			groundInfo.m_position.set(0, -2.0f, 0);
			hkpRigidBody* ground = new hkpRigidBody(groundInfo);
			m_world->addEntity(ground);
			ground->removeReference();

			groundInfo.m_shape->removeReference();
		}
		//{
		//	hkpRigidBody* ground = getBodyFromFile("./media/circuito.hkx", "circuito4");
		//	if(ground == HK_NULL)
		//		return;
		//	ground->getCinfo(groundInfo);
		//	groundInfo.m_position.set(0, -2.0f, 0);
		//	groundInfo.m_friction = 0.8f;
		//	groundInfo.m_motionType = hkpMotion::MOTION_FIXED;
		//	groundInfo.m_mass = 0;
		//	ground->removeReference();
		//	ground = new hkpRigidBody(groundInfo);
		//	m_world->addEntity(ground);
		//	ground->removeReference();
		//}

		halfExtentsGround = hkVector4(5, 0.5f, 5);
		{
			groundInfo.m_shape = new hkpBoxShape(halfExtentsGround, 0);
			groundInfo.m_position.set(10, -1, 0);
			groundInfo.m_rotation.setAxisAngle(hkVector4(0, 0, 1), (5*180/HK_REAL_PI));
			hkpRigidBody* ground = new hkpRigidBody(groundInfo);
			m_world->addEntity(ground);
			ground->removeReference();
			
			groundInfo.m_shape->removeReference();
		}
	}
}

void Game::createVehicle(hkpRigidBody* chassis) {
	m_vehicle = new hkpVehicleInstance(chassis);
	VehicleSetup setup;
	setup.buildVehicle(m_world, *m_vehicle);

	m_vehicle->addToWorld(m_world);

	m_world->addAction(m_vehicle);
}

void Game::createDisplayWheels(hkReal radius, hkReal thickness) {
	m_displayWheelId.setSize(m_vehicle->m_data->m_numWheels);

	for(int i = 0; i < m_displayWheelId.getSize(); i++) {
		int displayId = (i * 2) + 1;

		VehicleApiUtils::createDisplayWheels(m_tag, displayId);

		m_displayWheelId[i] = displayId;
	}
}

#include <Common/Base/keycode.cxx>

// we're not using any product apart from Havok Physics.
#undef HK_FEATURE_PRODUCT_AI
#undef HK_FEATURE_PRODUCT_ANIMATION
#undef HK_FEATURE_PRODUCT_CLOTH
#undef HK_FEATURE_PRODUCT_DESTRUCTION
#undef HK_FEATURE_PRODUCT_BEHAVIOR

// Also we're not using any serialization/versioning so we don't need any of these.
#define HK_EXCLUDE_FEATURE_SerializeDeprecatedPre700
#define HK_EXCLUDE_FEATURE_RegisterVersionPatches
#define HK_EXCLUDE_FEATURE_MemoryTracker

#include <Common/Base/Config/hkProductFeatures.cxx>